#!/usr/bin/env python
import rospy
import subprocess
import yaml
import os
import time
from service_launcher.srv import StartNode, StartNodeResponse

def load_config():
    """读取 YAML 配置文件"""
    try:
        with open("/home/cao/ros_ws/src/service_launcher/config/config.yaml", "r") as file:
            config = yaml.safe_load(file)
        return config
    except FileNotFoundError:
        rospy.logerr("config/config.yaml not found. Please ensure the config directory and file exist.")
        return None

def generate_unique_node_name(base_name):
    """根据时间戳生成唯一的节点名称"""
    timestamp = time.strftime("%Y%m%d-%H%M%S")
    return f"{base_name}_{timestamp}"

def handle_start_node(req):
    config = load_config()  # 加载配置文件
    if config is None:
        return StartNodeResponse(success=False, message="Failed to load configuration file.")

    service_name = req.node_name  # 获取请求中的服务名称

    # 遍历服务配置列表，查找匹配的服务
    for service in config.get('services', []):
        if service.get('name') == service_name:
            try:
                # 获取参数列表（如果有）
                params = service.get('params', [])
                working_dir = service.get('working_dir')  # 获取工作目录

                # 设置环境变量以避免当前环境的影响
                env = os.environ.copy()  # 获取当前的环境变量副本

                # 根据服务类型选择启动 rosrun、roslaunch 或 Python 脚本
                if service.get('type') == 'rosrun':
                    # 如果是 rosrun，构造命令并执行
                    command = ['rosrun', service.get('package'), service.get('node')] + params

                    # 动态生成唯一的节点名称
                    unique_node_name = generate_unique_node_name(service.get('node'))
                    command += ['_name', unique_node_name]  # 添加生成的节点名称作为参数

                    rospy.loginfo(f"Running command: {command}")

                    if working_dir:
                        rospy.loginfo(f"切换目录到: {working_dir}")
                        subprocess.Popen(command, cwd=working_dir, env=env)  # 启动节点
                    else:
                        subprocess.Popen(command, env=env)  # 启动节点

                    # 返回成功响应
                    return StartNodeResponse(success=True, message=f"Started {service_name} using rosrun with params {params} and node name {unique_node_name}.")

                elif service.get('type') == 'roslaunch':
                    # 如果是 roslaunch，构造命令并执行
                    command = ['roslaunch', '--screen', service.get('package'), service.get('launch_file')] + params

                    # 动态生成唯一的节点名称
                    unique_node_name = generate_unique_node_name(service.get('launch_file'))
                    command += ['__name', unique_node_name]  # 添加生成的节点名称作为参数

                    rospy.loginfo(f"Running command: {command}")  # 打印正在执行的命令
                    subprocess.Popen(command, env=env)  # 启动 launch 文件

                    # 返回成功响应
                    return StartNodeResponse(success=True, message=f"Started {service_name} using roslaunch with params {params} and node name {unique_node_name}.")

                elif service.get('type') == 'python':
                    # 如果是 python，构造命令并执行
                    script = service.get('script')
                    command = ['python', script] + params

                    # 动态生成唯一的节点名称
                    unique_node_name = generate_unique_node_name(script)
                    rospy.loginfo(f"Running Python script: {command}")

                    if working_dir:
                        rospy.loginfo(f"切换目录到: {working_dir}")
                        subprocess.Popen(command, cwd=working_dir, env=env)  # 启动 Python 脚本
                    else:
                        subprocess.Popen(command, env=env)  # 启动 Python 脚本

                    # 返回成功响应
                    return StartNodeResponse(success=True, message=f"Started {service_name} using Python script with params {params} and node name {unique_node_name}.")

            except Exception as e:
                # 如果启动过程中发生错误，返回失败响应
                return StartNodeResponse(success=False, message=f"Failed to start {service_name}: {str(e)}")

    # 如果没有找到对应的服务，返回失败响应
    return StartNodeResponse(success=False, message=f"Service {service_name} not found.")

def start_node_service():
    rospy.init_node('start_node_service')  # 初始化 ROS 节点
    service = rospy.Service('start_node', StartNode, handle_start_node)  # 注册服务
    rospy.loginfo("Ready to start individual ROS nodes, launch files, or Python scripts using services.")  # 服务启动时的日志信息
    rospy.spin()  # 保持服务持续运行

if __name__ == "__main__":
    start_node_service()  # 启动服务
